#include "log_task.h"

void register_all_variable(void){
	REGISTER_PARAM(GET_RAM_ADDR(led_config.led_left_init_state), TYPE_U8, LED_L_INIT_STATE);
	REGISTER_PARAM(GET_RAM_ADDR(led_config.led_left_freq), TYPE_U16, LED_L_FREQ);
	REGISTER_PARAM(GET_RAM_ADDR(led_config.led_right_init_state), TYPE_U8, LED_R_INIT_STATE);
	REGISTER_PARAM(GET_RAM_ADDR(led_config.led_right_freq), TYPE_U16, LED_R_FREQ);
	REGISTER_PARAM(GET_RAM_ADDR(now_normal_log_id), TYPE_U16, NOW_NORMAL_LOG_ID);
	REGISTER_PARAM(GET_RAM_ADDR(now_flight_log_id), TYPE_U16, NOW_FLIGHT_LOG_ID);
	REGISTER_PARAM(GET_RAM_ADDR(imu2.accel_x_offset), TYPE_FLOAT, IMU2_ACCEL_X_OFFSET);
	REGISTER_PARAM(GET_RAM_ADDR(imu2.accel_y_offset), TYPE_FLOAT, IMU2_ACCEL_Y_OFFSET);
	REGISTER_PARAM(GET_RAM_ADDR(imu2.accel_z_offset), TYPE_FLOAT, IMU2_ACCEL_Z_OFFSET);
	REGISTER_PARAM(GET_RAM_ADDR(imu2.gyro_x_offset), TYPE_FLOAT, IMU2_GYRO_X_OFFSET);
	REGISTER_PARAM(GET_RAM_ADDR(imu2.gyro_y_offset), TYPE_FLOAT, IMU2_GYRO_Y_OFFSET);
	REGISTER_PARAM(GET_RAM_ADDR(imu2.gyro_z_offset), TYPE_FLOAT, IMU2_GYRO_Z_OFFSET);
	REGISTER_PARAM(GET_RAM_ADDR(imu1.accel_x_offset), TYPE_FLOAT, IMU1_ACCEL_X_OFFSET);
	REGISTER_PARAM(GET_RAM_ADDR(imu1.accel_y_offset), TYPE_FLOAT, IMU1_ACCEL_Y_OFFSET);
	REGISTER_PARAM(GET_RAM_ADDR(imu1.accel_z_offset), TYPE_FLOAT, IMU1_ACCEL_Z_OFFSET);
	REGISTER_PARAM(GET_RAM_ADDR(imu1.gyro_x_offset), TYPE_FLOAT, IMU1_GYRO_X_OFFSET);
	REGISTER_PARAM(GET_RAM_ADDR(imu1.gyro_y_offset), TYPE_FLOAT, IMU1_GYRO_Y_OFFSET);
	REGISTER_PARAM(GET_RAM_ADDR(imu1.gyro_z_offset), TYPE_FLOAT, IMU1_GYRO_Z_OFFSET);
}

void register_monitor_variables(){
    MONITOR_VARIABLE(GET_RAM_ADDR(dart_attitude.accels[0]), TYPE_FLOAT, IMU_MIX_ACCEL_X);
    MONITOR_VARIABLE(GET_RAM_ADDR(dart_attitude.accels[1]), TYPE_FLOAT, IMU_MIX_ACCEL_Y);
    MONITOR_VARIABLE(GET_RAM_ADDR(dart_attitude.accels[2]), TYPE_FLOAT, IMU_MIX_ACCEL_Z);
    MONITOR_VARIABLE(GET_RAM_ADDR(dart_attitude.gyros[0]), TYPE_FLOAT, IMU_MIX_GYRO_X);
    MONITOR_VARIABLE(GET_RAM_ADDR(dart_attitude.gyros[1]), TYPE_FLOAT, IMU_MIX_GYRO_Y);
    MONITOR_VARIABLE(GET_RAM_ADDR(dart_attitude.gyros[2]), TYPE_FLOAT, IMU_MIX_GYRO_Z);
    MONITOR_VARIABLE(GET_RAM_ADDR(dart_attitude.imu_angle.pitch), TYPE_FLOAT, DART_PITCH);
    MONITOR_VARIABLE(GET_RAM_ADDR(dart_attitude.imu_angle.yaw), TYPE_FLOAT, DART_YAW);
    MONITOR_VARIABLE(GET_RAM_ADDR(dart_attitude.imu_angle.roll), TYPE_FLOAT, DART_ROLL);
}

static void calc_register_offset_table(){
    uint16_t  now_offset = 0;
    for(int i= 0; i< setting_variables.now_num; i++){
        setting_variables.index_to_offset[i] = now_offset;
        now_offset += SIZEOF(setting_variables.pack[i].type);
    }

    now_offset = 0;
    for(int i=0;i < monitor_var.now_num; i++){
        monitor_var.index_to_offset[i] = now_offset;
        now_offset += SIZEOF(monitor_var.monitor_vars[i].type);
    }
}

void log_task(void const * argument){
	register_all_variable();
	register_monitor_variables();
    calc_register_offset_table();
	while(1){
		file_system_controller();
		log_system_auto();
		switch(core_step){
		 	case CORE_PARAM_WAIT_INIT:
				if(log_system.log_system_state == LOG_SYSTEM_IDLE){
					core_step = CORE_RUN;
				}
		 		break;
		 	case CORE_RUN:
		 		break;
		 	default:
		 		break;
		}
		osDelay(2);
	}
}
